/**
 * Author: luoqi
 * Created Date: 2025-11-15 01:34:8
 * Last Modified: 2025-11-20 00:03:30
 * Modified By: luoqi at <**@****>
 * Copyright (c) 2025 <*****>
 * Description:
 */

#include <math.h>
#include "mc.h"

int mc_init(McObj *mc)
{
    if(!mc) {
        return -1;
    }
    mc->mod = MC_MOD_IDLE;
    mc->pos = 0.0f;
    mc->vel = 0.0f;
    mc->pref = 0.0f;
    mc->vref = 0.0f;
    pid_init(&mc->vpid, 0.0f, 0.0f, 0.0f, 0.0f);
    pid_init(&mc->ppid, 0.0f, 0.0f, 0.0f, 0.0f);
    lpf_1st_init(&mc->vlpf, 0.0f, 0.0f);
    lpf_1st_init(&mc->ovlpf, 0.0f, 0.0f);
    return 0;
}

int mc_mod_set(McObj *mc, McMod mod)
{
    if(!mc) {
        return -1;
    }
    mc->mod = mod;
    return 0;
}

int mc_ep_max_set(McObj *mc, float ep_max)
{
    if(!mc) {
        return -1;
    }
    mc->ep_max = ep_max;
    return 0;
}

int mc_pos_update(McObj *mc, float pos)
{
    if(!mc) {
        return -1;
    }

    mc->pos = pos;
    return 0;
}

int mc_vel_update(McObj *mc, float vel)
{
    if(!mc) {
        return -1;
    }
    mc->vel = lpf_1st_calc(&mc->vlpf, vel);
    return 0;
}

int mc_vpid_set(McObj *mc, float kp, float ki, float kd, float omax)
{
    if(!mc) {
        return -1;
    }
    pid_init(&mc->vpid, kp, ki, kd, omax);
    return 0;
}

int mc_ppid_set(McObj *mc, float kp, float ki, float kd, float omax)
{
    if(!mc) {
        return -1;
    }
    pid_init(&mc->ppid, kp, ki, kd, omax);
    return 0;
}

int mc_vlpf_set(McObj *mc, float fc, float fs)
{
    if(!mc) {
        return -1;
    }
    lpf_1st_init(&mc->vlpf, fc, fs);
    return 0;
}

int mc_ovlpf_set(McObj *mc, float fc, float fs)
{
    if(!mc) {
        return -1;
    }
    lpf_1st_init(&mc->ovlpf, fc, fs);
    return 0;
}

float mc_vloop(McObj *mc, float vref, float dt)
{
    if(!mc) {
        return 0.0f;
    }
    mc->vref = vref;
    float err = mc->vref - mc->vel;
    float v = pid_calc(&mc->vpid, err, dt);
    return lpf_1st_calc(&mc->ovlpf, v);
}

float mc_ploop(McObj *mc, float pref, float dt)
{
    if(!mc) {
        return 0.0f;
    }

    pref = fmodf(pref, 360.0f);
    
    if(pref < 0.0f) {
        pref += 360.0f;
    }

    mc->pref = pref;

    float err = mc->pref - mc->pos;

    if(err > 180.0f) {
        err -= 360.0f;
    } else if(err < -180.0f) {
        err += 360.0f;
    }

    if(fabsf(err) > mc->ep_max) {
        err = err > 0.0f ? mc->ep_max : -mc->ep_max;
    }

    return pid_calc(&mc->ppid, err, dt);
}
